/**
 * 
 */
package logic.algorithm;
import java.util.Vector;

import datamodel.Grid;
import datamodel.Grid.Neighbors;
import datamodel.GridElement;
import datamodel.GridElementAlgoState;
/**
 * 
 * @version 0.1
 *
 */
public class A_Star implements Algorithm {
	
	Grid grid;
	boolean solved = false;
	GridElement currentNode = null;

	/**
	 * @see logic.algorithm.Algorithm#init(datamodel.Grid)
	 */
	@Override
	public void init(Grid grid) {
		this.grid = grid;
		// First, set up the grid by marking all nodes as unvisited, setting their distances to the max value
		// and setting all predecessors to null
		for (int row = 0; row < this.grid.getRows(); row++){
			for (int column = 0; column < this.grid.getColumns(); column++){
				this.grid.getElementAt(row, column).setDistance(Integer.MAX_VALUE);
				this.grid.getElementAt(row, column).setAlgoState(GridElementAlgoState.NONE);
				this.grid.getElementAt(row, column).setPredecessor(null);
			}
		}
		// Then, set the start node to 0 distance
		this.grid.getStartElement().setDistance(0);
		this.currentNode = this.grid.getStartElement();

	}

	/**
	 * @see logic.algorithm.Algorithm#doNextStep()
	 */
	@Override
	public boolean doNextStep() {
		
		//** INITIALIZATION OF THE STEP **\\
		
		// First, we check what our current node is
		if (currentNode == null){
			// If the current node is undefined, it means that our algorithm is finished,
			// so we return false
			return false;
		} else if ((currentNode.getRow() == grid.getEndElement().getRow()) && 
				(currentNode.getColumn() == grid.getEndElement().getColumn())){
			currentNode.setAlgoState(GridElementAlgoState.LOOKED_AT);
			// If the current node is the same as the end node, we set solved to true
			// Because the graph has been solved
			solved = true;
			// And we return false, because the algorithm is finished
			return false;
		}
		// If the current node checks out, we continue initializing this
		// step of the algorithm
		
		// First, get the neighbor data of the current node
		Neighbors neighborData = this.grid.getNeighborsFrom(currentNode, true);
		// Then, get the way cost and neighbor vectors
		Vector<GridElement> neighbors = neighborData.getNeighbors();
		Vector<Integer> wayCosts = neighborData.getWayCosts();
		
		
		//** RELAXING TEH EDGES **\\
		//Now, relax the edges leading to the neighbors 
		for (int pos = 0; pos < neighbors.size(); pos++){
			// Assign variables for ease of use
			GridElement neighbor = neighbors.get(pos);
			int newDistance = currentNode.getDistance() + wayCosts.get(pos);
			
			if(neighbor == null){
				// If the neighbor is null, it is out of bounds of the map and therefore unimportant
				// so continue searching through the neighbors
				continue;
			} else if (lookedAt(neighbor)){
				// If the neighbor has already been visited, ignore it an
				// continue searching through the neighbors
				continue;
			} else if (wayCosts.get(pos) == Integer.MAX_VALUE){
				// If the way cost to the nighbor is the maximum, then it is a blocked field
				// so the neighbor should be marked as looked at, and the search through the neighbors should conitinue
				neighbor.setAlgoState(GridElementAlgoState.LOOKED_AT);
				continue;
			} else if (neighbor.getDistance() == Integer.MAX_VALUE){
				// If the distance is the max value, set the distance to the newDistance value and
				// set the current node as the neighbor's predecessor
				neighbor.setDistance(newDistance);
				// Since the setDistance() method marks the node as visited, revert it to NONE
				neighbor.setAlgoState(GridElementAlgoState.NONE);
				neighbor.setPredecessor(currentNode);
			} else if (newDistance < neighbor.getDistance()){
				// If the new distance value is smaller than the neighbor's original distance,
				// set the distance to the newDistance value and 
				// set the current node as the neighbor's predecessor
				neighbor.setDistance(newDistance);
				// Since the setDistance() method marks the node as visited, revert it to NONE
				neighbor.setAlgoState(GridElementAlgoState.NONE);
				neighbor.setPredecessor(currentNode);
			}
		}
		
		//** AFTER-STEP CLEAN-UP **\\
		// Since all of the neighbors of the current node have been inspected, mark the current node as visited
		currentNode.setAlgoState(GridElementAlgoState.LOOKED_AT);
		// Finally, set the current node to the next closest Grid Element
		currentNode = getClosestGridElement();
		// and return true, indicating that there are more steps left to do
		return true;
		
	}

	/**
	 * @see logic.algorithm.Algorithm#isSolved()
	 */
	@Override
	public boolean isSolved() {
		return solved;
	}
	
	/**
	 * Searches all elements in the grid and returns the GridElement with the shortest distance value
	 * that hasn't been visited yet
	 * @return the GridElement with the shortest distance value, null if there are no unvisited elements
	 */
	private GridElement getClosestGridElement(){
		// First create fields for the comparison
		double smallestDistance = Double.MAX_VALUE;
		GridElement closestElement = null;
		// then, run through all the nodes in the grid
		for (int row = 0; row < this.grid.getRows(); row++){
			for (int column = 0; column < this.grid.getColumns(); column++){
				// Assign a variable for ease of use
				GridElement element = this.grid.getElementAt(row, column);
				if (element.getDistance() == Integer.MAX_VALUE){
					// if the element has the max distance, keep looking
					continue;
				} else if(lookedAt(element)){
					//if the element has already been marked as visited, keep looking
					continue;
				} else if(element.getDistance() + getWeightedDistance(element) < smallestDistance){
					// if the element has a distance smaller than all others checked so far,
					// set it as the closest element
					smallestDistance = element.getDistance() + getWeightedDistance(element);
					closestElement = element;
				} else {
					// Otherwise, continue searching through the grid
					continue;
				}
			}
		}
		// Once the search is over, return the result
		return closestElement;
		
	}
	
	protected boolean lookedAt(GridElement e){
		GridElementAlgoState elementAlgoState = e.getAlgoState();
		switch(elementAlgoState){
		case LOOKED_AT : return true;
		default : return false;
		}
	}
	
	/**
	 * Finds the weighted distance of a GridElement e from the end node
	 * @param e is the GridElement whose distance from the end node is needed
	 * @return the weighted distance of e from the end node
	 */
	private double getWeightedDistance(GridElement e){
		GridElement end = grid.getEndElement();
		double xDist = Math.abs(e.getColumn() - end.getColumn());
		double yDist = Math.abs(e.getRow() - end.getRow());
		double rawDistance = (Math.sqrt(Math.pow(xDist, 2.0) + Math.pow(yDist, 2.0)));
		double angle = Math.acos(xDist / rawDistance) / Math.PI;
		if (angle < 0.5){
			return rawDistance * (4 * (1 + angle));
		} else if(angle > 0.5){
			return rawDistance * (4 * (1.5 - angle));
		} else {
			return rawDistance * (4 * 1.5);
		}
	}
	
}
